import json
import time
import threading
from hqyj_mqtt import Mqtt_Clt
import base64
import numpy as np
import cv2
from predict import predict_yolo

# 推杆常量
load_rod = "first"  # 加载推杆
ripe_rod = "second"  # 成熟果推杆
half_ripe_rod = "third"  # 半成熟果推杆
raw_rod = "fourth"  # 生果推杆

# 延迟时间
DELAY = {"ripe": 0.85, "half-ripe": 1.78, "raw": 2.65}

# 推杆参数
PUSH_DURATION = 0.33  # 单次推杆持续时间
LOAD_INTERVAL = 0.25  # 加载推杆推送间隔

def push_pull(rod):
    """单次推杆动作：推出→收回"""
    mqtt_client.send_json_msg(json.dumps({"rod_control": f"{rod}_push"}))
    time.sleep(PUSH_DURATION)
    mqtt_client.send_json_msg(json.dumps({"rod_control": f"{rod}_pull"}))

def continuous_push(rod):
    """加载推杆：固定间隔推送货物"""
    while True:
        push_pull(rod)
        time.sleep(LOAD_INTERVAL)

def delayed_push(target_rod, target_type):
    """延迟推送：经过相应时间后执行推送"""
    time.sleep(DELAY[target_type])  # 等待延迟时间
    push_pull(target_rod)
    print(f"执行推送：{target_type}果 → {target_rod}推杆")

if __name__ == '__main__':
    # 初始化MQTT与设备
    mqtt_client = Mqtt_Clt("127.0.0.1", 21883, "bb", "aa", 60)
    print("系统启动")
    #初始化传送带和推杆
    mqtt_client.control_device("conveyor", 'run')
    mqtt_client.control_device("rod_control", "all_pull")

    # 启动加载推杆线程
    threading.Thread(target=continuous_push, args=(load_rod,), daemon=True).start()

    # 处理图像识别
    while True:
        msg = mqtt_client.mqtt_queue.get()

        # 收到图像→识别货物类型并启动延迟推送
        if 'image' in msg:
            # 解码图像并识别
            image_data = base64.b64decode(msg['image'])
            image = cv2.imdecode(np.frombuffer(image_data, np.uint8), cv2.IMREAD_COLOR)
            goods_type = predict_yolo(image)
            print(f"识别结果：{goods_type}")

            # 识别有效类型→启动延迟推送线程
            if goods_type in DELAY:
                if goods_type == "ripe":
                    threading.Thread(target=delayed_push, args=(ripe_rod, goods_type), daemon=True).start()
                elif goods_type == "half-ripe":
                    threading.Thread(target=delayed_push, args=(half_ripe_rod, goods_type), daemon=True).start()
                elif goods_type == "raw":
                    threading.Thread(target=delayed_push, args=(raw_rod, goods_type), daemon=True).start()
                print(f"延迟任务设置：{goods_type}果将在{DELAY[goods_type]}秒后推送")